/*
    WLR740 补点算法
*/

#include <mutex>
#include <unordered_map>


#include <vanjee_driver/driver/decoder/wlr740/decoder/one_point_info_manage.hpp>
#include <vanjee_driver/driver/decoder/decoder.hpp>

namespace vanjee
{
    namespace lidar
    {
        class Filterate
        {
        private:
            static Filterate* _;
            static std::mutex this_mutex_;

            Trigon trigon_;

            bool algorithm_enable_ = false;
            bool blank_point_compen_enable_ = false;

            bool outlier_enable_ = false;
            int outlier_num_ = 4;
            double outlier_radius_ = 1.5;

            bool close_range_outliers_enable_ = false;
            int close_range_outliers_num_ = 10;
            double close_range_outliers_radius_ = 1.5;

            int start_angle_ = 0;
            unsigned int full_search_scope_ = 5000; //完全搜索范围  此范围内的点 不进行跳点查找

            int corrspond_[8][6] = {
                {1,9 ,17,25,33,41},
                {5,13,21,29,37,45},
                {2,10,18,26,34,42},
                {6,14,22,30,38,46},
                {3,11,19,27,35,43},
                {7,15,23,31,39,47},
                {4,12,20,28,36,44},
                {8,16,24,32,40,48}};
            int line_group1_[48];
            std::vector<std::vector<int>> corrspond_line_;
            int h_resolution_ = 1; // 分辨率
        public:

            Filterate();
            static Filterate* GetInstance();

            std::vector<int> GetCorrspondLine(int passage);

            void SetHResolution(int val);

            void SetStartAngle(int val) { start_angle_ = val; }    // 分辨率为0.1 时配置为 1  当分辨率为0.2时 收到的为奇数角配置为1 偶数角配置为0，
            inline int GetStartAngle() { return start_angle_; }
            unsigned int GetFullSearchScope() const& {return full_search_scope_; }


            void SetAlgorithmEnb(bool val) { algorithm_enable_ = val; }

            void SetBlankPointCompenEnb(bool val) { blank_point_compen_enable_ = val; }

            void SetOutlierEnb(bool val) { outlier_enable_ = val; }

            void SetCloseRangeOutliersEnb(bool val) { close_range_outliers_enable_ = val; }

            /// @brief 点云算法处理 注意：和回波模式要进行对应否则可能会造成部分点云不显示
            /// @param points  当前一圈的点云
            /// @param pre_points 上一圈的点云
            /// @param mode 1-只处理最强一重 2-只处理最后一重 3-最强最后都处理 
            void filter(OnePointInfoManage &points,OnePointInfoManage &pre_points,int mode = 3);
          
            void BlankPointCompenV5(OnePointInfoManage &points);

            void BlankPointCompenV5Process(OnePointInfo& point, OnePointInfoManage& points);

            void _40BlankPointCompenProcessV5(OnePointInfoManage &points);

            void _40BlankPointCompenProcessV6(OnePointInfoManage &points);

            void ExpansionProcess(OnePointInfoManage& points);

            inline void FilterNoiseV2(OnePointInfoManage &points,OnePointInfoManage &pre_points);

            inline void FilterNoiseProcess(int init, OnePointInfo &point,OnePointInfo &pre_point,OnePointInfoManage &points);

            inline void close_range_outliers(OnePointInfoManage &points,OnePointInfoManage &pre_points);

            inline void CloseRangeOutliersProcess(int init, OnePointInfo& point,OnePointInfoManage &points);

            /// @brief 点云算法处理 注意：和回波模式要进行对应否则可能会造成部分点云不显示
            /// @param points  当前一圈的点云
            /// @param pre_points 上一圈的点云
            /// @param mode 1-只处理最强一重 2-只处理最后一重 3-最强最后都处理 
            inline void publicIterate(OnePointInfoManage &points,OnePointInfoManage &pre_points,int mode);

        private:
            void DoublePointAvg(OnePointInfo& src, OnePointInfo& p1,OnePointInfo& p2);
            inline void Angle38Check(int& angle1, int& angle2, int cuLine, int check_line);
        };
        Filterate* Filterate::_;
        std::mutex Filterate::this_mutex_;
        Filterate::Filterate()
        {
            for(int i = 0; i < 48;i++)
            {
                line_group1_[i] = i;   // LD1-LD16 对应 1-48线
                corrspond_line_.push_back(GetCorrspondLine(i+1));
            }
        }


        Filterate* Filterate::GetInstance()
        {
            //  这里使用了两个 if 判断语句的技术称为双检锁；好处是，只有判断指针为空的时候才加锁，
            //  避免每次调用 GetInstance的方法都加锁，锁的开销毕竟还是有点大的。
            if (_ == nullptr)
            {
                std::unique_lock<std::mutex> lock(this_mutex_); // 加锁
                if (_ == nullptr)
                {
                    auto temp = new (std::nothrow) Filterate();
                    _ = temp;
                }
            }

            return _;
        }

        void Filterate::SetHResolution(int val)
        {
            h_resolution_ = val;
        }

        std::vector<int> Filterate::GetCorrspondLine(int passage)
        {
            std::vector<int> res;
            for(int row = 0;row < 8;row++)
            {
                for(int col = 0;col < 6;col++)
                {
                    if(passage == corrspond_[row][col])
                    {
                        for(int col1 = 0;col1 < 6;col1++)
                        {
                            int RotatingMirror = 0,l_LineNo = 0;

                            l_LineNo = (corrspond_[row][col1] - 1)*3+RotatingMirror + 2;
                            res.push_back(l_LineNo);
                            RotatingMirror++;

                            l_LineNo = (corrspond_[row][col1] - 1)*3+RotatingMirror + 0;
                            res.push_back(l_LineNo);
                            RotatingMirror++;

                            l_LineNo = (corrspond_[row][col1] - 1)*3+RotatingMirror + 1;
                            res.push_back(l_LineNo);

                        }
                        return res;
                    }
                }
            }
            return res;
        }



        inline void Filterate::filter(OnePointInfoManage &points,OnePointInfoManage &pre_points, int mode)
        {

            if(!algorithm_enable_)
                return;

            //auto start = std::chrono::steady_clock::now();

            // if(blank_point_compen_enable_)
            // {
            //     _40BlankPointCompenProcessV6(points);
            //     BlankPointCompenV5(points);
            // }
            // if(outlier_enable_)
            // {
            //     FilterNoiseV2(points, pre_points); // 半径滤波 && 近距离0.2弱光分辨率处理
            // }
            
            // if(close_range_outliers_enable_) // 近距离0.2弱光分辨率处理
            // {
            //     close_range_outliers(points, pre_points);
            // }

            // 膨胀算法 默认关闭
            //ExpansionProcess(points); 

            publicIterate(points, pre_points, mode);

            // auto end = std::chrono::steady_clock::now();
            // std::chrono::duration<double, std::micro> elapsed1 = end - start;

            // std::stringstream logc;
            // logc << "总耗时：" << elapsed1.count()
            // << " 微秒";
            // std::cout << logc.str();
        }

        void Filterate::publicIterate(OnePointInfoManage &points,OnePointInfoManage &pre_points,int mode)
        {
            if(mode == 1 || mode == 3)
            {
                for(int line_id = 1; line_id <= 144;line_id++)
                {
                    for(int angle = start_angle_; angle < 1200;angle+=h_resolution_)
                    {
                        OnePointInfo& point = points.GetData(1,line_id,angle);
                        OnePointInfo& pre_point = pre_points.GetData(1,line_id,angle);

                        if(line_id == 144 && angle == 1199)
                        {
                            FilterNoiseProcess(1, point, pre_point, points);
                            CloseRangeOutliersProcess(1, point, points);
                        }
                        else
                        {
                            FilterNoiseProcess(0, point, pre_point, points);
                            CloseRangeOutliersProcess(0,point,points);
                            BlankPointCompenV5Process(point,points);
                        }
                    }
                }
            }
            if(mode == 2 || mode == 3)
            {
                for(int line_id = 1; line_id <= 144;line_id++)
                {
                    for(int angle = start_angle_; angle < 1200;angle+=h_resolution_)
                    {
                        OnePointInfo& point = points.GetData(0,line_id,angle);
                        OnePointInfo& pre_point = pre_points.GetData(0,line_id,angle);

                        if(line_id == 144 && angle == 1199)
                        {
                            FilterNoiseProcess(1, point, pre_point, points);
                            CloseRangeOutliersProcess(1, point, points);
                        }
                        else
                        {
                            FilterNoiseProcess(0, point, pre_point, points);
                            CloseRangeOutliersProcess(0,point,points);
                            BlankPointCompenV5Process(point,points);
                        }
                    }
                }
            }         
        }


        inline void Filterate::BlankPointCompenV5(OnePointInfoManage &points)
        {
            static OnePointInfo left_point; // 左边点
            static OnePointInfo right_point; // 右边点
            static int points_zero_flag = 0;
            static int points_zero_cn = 0;
            static int config_continuous_zero_cn= 5; // 连续5个点为 0 则不进行补点，--> 因此 只支持 4个点为 0 值 才能补
            static int config_interval_distance = 200; // 20cm
            if(h_resolution_ == 1)
            {
                config_continuous_zero_cn= 5;
            }
            else
            {
                config_continuous_zero_cn= 10;
            }

            for(int line_id = 1; line_id <= 144;line_id++)
            {
                points_zero_flag = 0;
                points_zero_cn = 0;
                for(int angle = 1; angle < 1200;angle+=h_resolution_)
                {
                    OnePointInfo& point = points.GetData(1,line_id,angle);

                    // 如果当前的点的距离不为0
                    if( point.distance !=0 )
                    {
                        // 如果之前有点为 0 则更新左边点
                        if(points_zero_flag == 1)
                        {
                            right_point = point;

                            // 判断 和 左点间隔 多少个 点
                            int interval_index = right_point.index - left_point.index;
                            int interval_distance = std::abs((int)right_point.distance - (int)left_point.distance);
                            int zero_point_angle = left_point.h_angle + h_resolution_;

                            if( (interval_index > 0) &&
                                (interval_index < config_continuous_zero_cn) &&  // 防止 有丢帧 因此 需要判断 左右点的间隔 距离
                                (interval_distance <= config_interval_distance))
                            {
                                int avg_distance = (left_point.distance + right_point.distance) /2;
                                int avg_pulsewidth = (left_point.low_thres_pulsewidth + right_point.low_thres_pulsewidth) /2;
                                int intensity = (left_point.intensity + right_point.intensity) /2;
                                // float r = (left_point.r + right_point.r) / 2;
                                // float g = (left_point.g + right_point.g) / 2;
                                // float b = (left_point.b + right_point.b) / 2;

                                //while(--interval_index)
                                for(int i = 0; i < interval_index; i+= h_resolution_)
                                {
                                    // 注意 ：用 OnePointInfo& src 获取值 GetData数据line为0 时 不要去改变 src数据 因为返回是全局变量
                                    OnePointInfo& src = points.GetData(1,point.line_id,zero_point_angle);
                                    zero_point_angle += h_resolution_;
                                    // 这里 有个 问题 丢帧 导致丢数据时 vazimuth azimuth 是为 0 因此 无法正确补点 ，硬要补 也不能不能补 无非就是要计算 水平角即可
                                    if(src.line_id == 0)
                                        continue;
                                    double cos_rid_sin = COS((int)src.vazimuth) * SIN((int)src.azimuth * 10) *0.001;
                                    double cos_rid_cos = COS((int)src.vazimuth) * COS((int)src.azimuth * 10) *0.001;
                                    double sin_rid_thousandth = SIN((int)src.vazimuth) *0.001;

                                    src.distance = avg_distance;
                                    src.low_thres_pulsewidth = avg_pulsewidth;
                                    src.intensity = intensity;
                                    src.y = src.distance * cos_rid_sin;//新坐标系计算方式，逆时针改顺时针的方式
                                    src.x = src.distance * cos_rid_cos;
                                    src.z =  src.distance * sin_rid_thousandth;//不补偿
                                    src.blank_point_compen = 1;
                                    //if(!BlankPointCompenHighlight)
                                    //{
                                    //    src.r = r;
                                    //    src.g = g;
                                    //    src.b = b;
                                    //}
                                    // else
                                    // {
                                    //     src.r = 0;
                                    //     src.g = 255;
                                    //     src.b = 0;
                                    // }
                                }
                            }
                            else if((interval_index > 0) &&
                                    (interval_index < 30))
                            {

                                for(int i = 0; i < interval_index; i+= h_resolution_)
                                {
                                    // 注意 ：用 OnePointInfo& src 获取值 GetData数据line为0 时 不要去改变 src数据 因为返回是全局变量
                                    OnePointInfo& src = points.GetData(1,point.line_id,zero_point_angle);

                                    int up_diff = 1;
                                    int down_diff = 1;
                                    if(point.strong_weak == 2)
                                    {
                                        // 弱光的上下点的获取需要单独区分
                                        if(src.line_id == 23)
                                        {
                                            up_diff = 2;
                                            down_diff = 3;
                                        }
                                        else if(src.line_id == 25)
                                        {
                                            up_diff = 3;
                                            down_diff = 2;
                                        }
                                        else if(src.line_id == 46)
                                        {
                                            up_diff = 5;
                                            down_diff = 3;
                                        }
                                        else if(src.line_id == 51)
                                        {
                                            up_diff = 3;
                                            down_diff = 5;
                                        }
                                        else if(src.line_id == 72)
                                        {
                                            up_diff = 2;
                                            down_diff = 3;
                                        }
                                        else if(src.line_id == 74)
                                        {
                                            up_diff = 3;
                                            down_diff = 2;
                                        }
                                        else if(src.line_id == 95)
                                        {
                                            up_diff = 2;
                                            down_diff = 3;
                                        }
                                        else if(src.line_id == 118)
                                        {
                                            up_diff = 5;
                                            down_diff = 3;
                                        }
                                        else if(src.line_id == 123)
                                        {
                                            up_diff = 3;
                                            down_diff = 5;
                                        }
                                        else
                                        {
                                            up_diff = 3;
                                            down_diff = 3;
                                        }
                                    }

                                    if( (src.line_id!=0) &&
                                        ((src.line_id + up_diff) >= 1 && (src.line_id + up_diff) <= 144) &&
                                        ((src.line_id - down_diff) >= 1 && (src.line_id - down_diff) <= 144))
                                    {
                                        OnePointInfo& up_point = points.GetData(1,src.line_id + up_diff,zero_point_angle);
                                        OnePointInfo& down_point = points.GetData(1,src.line_id - down_diff,zero_point_angle);

                                        if((up_point.distance == 0) || (down_point.distance == 0))
                                            continue;

                                        interval_distance = std::abs((int)up_point.distance - (int)down_point.distance);
                                        if(interval_distance <= config_interval_distance)
                                        {
                                            int avg_distance = (up_point.distance + down_point.distance) /2;
                                            int avg_pulsewidth = (up_point.low_thres_pulsewidth + down_point.low_thres_pulsewidth) /2;
                                            int intensity = (up_point.intensity + down_point.intensity) /2;
                                            // float r = (up_point.r + down_point.r) / 2;
                                            // float g = (up_point.g + down_point.g) / 2;
                                            // float b = (up_point.b + down_point.b) / 2;

                                            double cos_rid_sin = COS((int)src.vazimuth) * SIN((int)src.azimuth * 10) *0.001;
                                            double cos_rid_cos = COS((int)src.vazimuth) * COS((int)src.azimuth * 10) *0.001;
                                            double sin_rid_thousandth = SIN((int)src.vazimuth) *0.001;

                                            src.distance = avg_distance;
                                            src.low_thres_pulsewidth = avg_pulsewidth;
                                            src.intensity = intensity;
                                            src.y = src.distance * cos_rid_sin;//新坐标系计算方式，逆时针改顺时针的方式
                                            src.x = src.distance * cos_rid_cos;
                                            src.z =  src.distance * sin_rid_thousandth;//不补偿
                                            src.blank_point_compen = 1;
                                            //if(!BlankPointCompenHighlight)
                                            //{
                                            //    src.r = r;
                                            //    src.g = g;
                                            //    src.b = b;
                                            //}
                                            //else
                                            // {
                                            //     src.r = 0;
                                            //     src.g = 255;
                                            //     src.b = 0;
                                            // }

                                        }
                                    }
                                    zero_point_angle += h_resolution_;

                                }

                                // 查看 上点和下点

                            }

                            left_point = point; // 更新右边点
                            points_zero_flag = 0; //补点完成 标志位 置 0
                            points_zero_cn = 0;
                        }
                        else
                        {
                            left_point = point; // 更新右边点
                            points_zero_flag = 0;
                            points_zero_cn = 0;
                        }
                    }
                    else
                    {
                        // 如果当前的点的 距离为 0

                        // 检查他的 上点 和下点  上下点 都有值 则 补点


                        points_zero_flag = 1; // 将标志位 置 1

                        if(points_zero_flag == 1)
                        {
                            points_zero_cn++;

                            // 如果超过了 配置 补点最大值 则  不能补点
                            if(points_zero_cn >= 30)
                            {
                                points_zero_flag = 0; // 清空 标志位
                                points_zero_cn = 0;   // 清空 连续 0点次数
                            }
                        }
                    }


                }
            }
        }

        inline void Filterate::BlankPointCompenV5Process(OnePointInfo& point, OnePointInfoManage& points)
        {
            static OnePointInfo left_point; // 左边点
            static OnePointInfo right_point; // 右边点
            static int points_zero_flag = 0;
            static int points_zero_cn = 0;
            static int config_continuous_zero_cn= 5; // 连续5个点为 0 则不进行补点，--> 因此 只支持 4个点为 0 值 才能补
            static int config_interval_distance = 300; // 20cm

            static int PreLine = 0;

            if(!blank_point_compen_enable_)
                return;

            if(h_resolution_ == 1)
            {
                config_continuous_zero_cn= 5;
            }
            else
            {
                config_continuous_zero_cn= 10;
            }

            if(point.line_id != PreLine)
            {
                points_zero_flag = 0;
                points_zero_cn = 0;
                PreLine = point.line_id;
            }

            // 如果当前的点的距离不为0
            if( point.distance !=0 )
            {
                // 如果之前有点为 0 则更新左边点
                if(points_zero_flag == 1)
                {
                    right_point = point;

                    // 判断 和 左点间隔 多少个 点
                    int interval_index = right_point.index - left_point.index;
                    int interval_distance = std::abs((int)right_point.distance - (int)left_point.distance);
                    int zero_point_angle = left_point.h_angle + h_resolution_;

                    if( (interval_index > 0) &&
                        (interval_index <= config_continuous_zero_cn) &&  // 防止 有丢帧 因此 需要判断 左右点的间隔 距离
                        (interval_distance <= config_interval_distance))
                    {
                        int avg_distance = (left_point.distance + right_point.distance) /2;
                        int avg_pulsewidth = (left_point.low_thres_pulsewidth + right_point.low_thres_pulsewidth) /2;
                        int intensity = (left_point.intensity + right_point.intensity) /2;
                        float r = (left_point.r + right_point.r) / 2;
                        float g = (left_point.g + right_point.g) / 2;
                        float b = (left_point.b + right_point.b) / 2;

                        //while(--interval_index)
                        for(int i = 0; i < interval_index-1; i+= h_resolution_)
                        {
                            // 注意 ：用 OnePointInfo& src 获取值 GetData数据line为0 时 不要去改变 src数据 因为返回是全局变量
                            OnePointInfo& src = points.GetData(point.echo_type,point.line_id,zero_point_angle);
                            zero_point_angle += h_resolution_;
                            // 这里 有个 问题 丢帧 导致丢数据时 vazimuth azimuth 是为 0 因此 无法正确补点 ，硬要补 也不能不能补 无非就是要计算 水平角即可
                            if(src.line_id == 0)
                                continue;
                            double cos_rid_sin = COS((int)src.vazimuth) * SIN((int)src.azimuth * 10) *0.001;
                            double cos_rid_cos = COS((int)src.vazimuth) * COS((int)src.azimuth * 10) *0.001;
                            double sin_rid_thousandth = SIN((int)src.vazimuth) *0.001;

                            src.distance = avg_distance;
                            src.low_thres_pulsewidth = avg_pulsewidth;
                            src.intensity = intensity;
                            src.y = src.distance * cos_rid_sin;//新坐标系计算方式，逆时针改顺时针的方式
                            src.x = src.distance * cos_rid_cos;
                            src.z =  src.distance * sin_rid_thousandth;//不补偿
                            src.blank_point_compen = 1;
                            src.non_filterable = 1; // 补点默认 不当作离群点
                            src.close_range_outliers = 1; //

                        }
                    }
                    else if((interval_index > 0) &&
                            (interval_index < 30))
                    {

                        for(int i = 0; i < interval_index; i+= h_resolution_)
                        {
                            // 注意 ：用 OnePointInfo& src 获取值 GetData数据line为0 时 不要去改变 src数据 因为返回是全局变量
                            OnePointInfo& src = points.GetData(point.echo_type,point.line_id,zero_point_angle);

                            int up_diff = 1;
                            int down_diff = 1;
                            if(point.strong_weak == 2)
                            {
                                // 弱光的上下点的获取需要单独区分
                                if(src.line_id == 23)
                                {
                                    up_diff = 2;
                                    down_diff = 3;
                                }
                                else if(src.line_id == 25)
                                {
                                    up_diff = 3;
                                    down_diff = 2;
                                }
                                else if(src.line_id == 46)
                                {
                                    up_diff = 5;
                                    down_diff = 3;
                                }
                                else if(src.line_id == 51)
                                {
                                    up_diff = 3;
                                    down_diff = 5;
                                }
                                else if(src.line_id == 72)
                                {
                                    up_diff = 2;
                                    down_diff = 3;
                                }
                                else if(src.line_id == 74)
                                {
                                    up_diff = 3;
                                    down_diff = 2;
                                }
                                else if(src.line_id == 95)
                                {
                                    up_diff = 2;
                                    down_diff = 3;
                                }
                                else if(src.line_id == 118)
                                {
                                    up_diff = 5;
                                    down_diff = 3;
                                }
                                else if(src.line_id == 123)
                                {
                                    up_diff = 3;
                                    down_diff = 5;
                                }
                                else
                                {
                                    up_diff = 3;
                                    down_diff = 3;
                                }
                            }

                            if( (src.line_id!=0) &&
                                ((src.line_id + up_diff) >= 1 && (src.line_id + up_diff) <= 144) &&
                                ((src.line_id - down_diff) >= 1 && (src.line_id - down_diff) <= 144))
                            {
                                OnePointInfo& up_point = points.GetData(point.echo_type,src.line_id + up_diff,zero_point_angle);
                                OnePointInfo& down_point = points.GetData(point.echo_type,src.line_id - down_diff,zero_point_angle);

                                if((up_point.distance == 0) || (down_point.distance == 0))
                                    continue;

                                interval_distance = std::abs((int)up_point.distance - (int)down_point.distance);
                                if(interval_distance <= config_interval_distance)
                                {
                                    int avg_distance = (up_point.distance + down_point.distance) /2;
                                    int avg_pulsewidth = (up_point.low_thres_pulsewidth + down_point.low_thres_pulsewidth) /2;
                                    int intensity = (up_point.intensity + down_point.intensity) /2;
                                    float r = (up_point.r + down_point.r) / 2;
                                    float g = (up_point.g + down_point.g) / 2;
                                    float b = (up_point.b + down_point.b) / 2;

                                    double cos_rid_sin = COS((int)src.vazimuth) * SIN((int)src.azimuth * 10) *0.001;
                                    double cos_rid_cos = COS((int)src.vazimuth) * COS((int)src.azimuth * 10) *0.001;
                                    double sin_rid_thousandth = SIN((int)src.vazimuth) *0.001;

                                    src.distance = avg_distance;
                                    src.low_thres_pulsewidth = avg_pulsewidth;
                                    src.intensity = intensity;
                                    src.y = src.distance * cos_rid_sin;//新坐标系计算方式，逆时针改顺时针的方式
                                    src.x = src.distance * cos_rid_cos;
                                    src.z =  src.distance * sin_rid_thousandth;//不补偿
                                    src.blank_point_compen = 1;
                                    src.non_filterable = 1; // 补点默认不作为离群点
                                    src.close_range_outliers = 1; //

                                }
                            }
                            zero_point_angle += h_resolution_;

                        }

                        // 查看 上点和下点

                    }

                    left_point = point; // 更新右边点
                    points_zero_flag = 0; //补点完成 标志位 置 0
                    points_zero_cn = 0;
                }
                else
                {
                    left_point = point; // 更新左边点
                    points_zero_flag = 0;
                    points_zero_cn = 0;
                }
            }
            else
            {
                // 如果当前的点的 距离为 0

                // 检查他的 上点 和下点  上下点 都有值 则 补点


                points_zero_flag = 1; // 将标志位 置 1

                if(points_zero_flag == 1)
                {
                    points_zero_cn++;

                    // 如果超过了 配置 补点最大值 则  不能补点
                    if(points_zero_cn >= 30)
                    {
                        points_zero_flag = 0; // 清空 标志位
                        points_zero_cn = 0;   // 清空 连续 0点次数
                    }
                }
            }

        }


        inline void Filterate::_40BlankPointCompenProcessV5(OnePointInfoManage &points)
        {

            OnePointInfo left_high_point; // 从入栈的顺序来看 先入栈的为左 后入栈 的为右
            OnePointInfo rigth_high_point;
            int continuous = 2;
            for(int line_id = 1; line_id < 145;line_id++) // 144 线 * 1600水平角度
            {
                std::vector<OnePointInfo> repaired;
                for(unsigned int angle = 0; angle < 1200;angle++)
                {
                    OnePointInfo& tem = points.GetData(1,line_id,angle);

                    // 130米后的鬼影
                    if(tem.distance > 130000)
                    {
                        if(tem.low_thres_pulsewidth > 19000) // 脉宽大于 19ns 检查同时发光的LD
                        {
                            std::vector<int> lines = corrspond_line_[tem.ld_id-1];
                            //int MirrorNo = tem.Mirror;
                            int h_angle = tem.h_angle;
                            for(int idx = 0;idx < lines.size();idx++)
                            {
                                if(tem.line_id != lines[idx] && (lines[idx] > 85)) // 自己就不用检测了
                                {
                                    long Key = points.GetIndex(1,lines[idx],h_angle);
                                    if(Key == -1)
                                        continue;
                                    auto& p = points.GetDataIt(Key);

                                    if(p.low_thres_pulsewidth < 9000 && std::abs((int)p.distance - (int)tem.distance) < 300)
                                    {
                                        //if(GhostHighlight)
                                        //{
                                        //    p.r = GhostHighlightColor.redF();
                                        //    p.g = GhostHighlightColor.greenF();
                                        //    p.b = GhostHighlightColor.blueF();
                                        //}
                                        //else
                                        {
                                            p.distance = 0;
                                            p.x = 0;
                                            p.y = 0;
                                            p.z = 0;
                                        }
                                    }
                                }
                            }
                        }
                    }//end 13米鬼影



                    if(tem.x == 0 && tem.y == 0 && tem.z == 0)
                    {
                        continue;
                    }

                    if(tem.low_thres_pulsewidth > 25000)
                    {
                        if(continuous == 1)
                        {
                            rigth_high_point = tem;

                            for(int i = 0; i < repaired.size(); i++)
                            {

                                OnePointInfo p1;
                                OnePointInfo p2;
                                bool ok = false;
                                if((std::abs((int)rigth_high_point.distance - (int)left_high_point.distance)  < 200))
                                {
                                    p1 = rigth_high_point;
                                    p2 = left_high_point;
                                    ok = true;
                                }

                                if(ok)
                                {
                                    int diff = std::abs((int)p1.distance - (int)repaired[i].distance);
                                    if(diff >= 38000 && diff <= 42000)
                                    {
                                        DoublePointAvg(repaired[i],p1,p2);
                                        repaired[i].blank_point_compen = 1;
                                        //if(!BlankPointCompenHighlight)
                                        //{
                                            repaired[i].r = (p2.r + p1.r) / 2;
                                            repaired[i].g = (p2.g + p1.g) / 2;
                                            repaired[i].b = (p2.b + p1.b) / 2;
                                        // }
                                        // else
                                        // {
                                        //     repaired[i].r = 255;
                                        //     repaired[i].g = 0;
                                        //     repaired[i].b = 0;
                                        // }

                                        points.insert(repaired[i].index,repaired[i]);
                                    }
                                }
                            }
                        }

                        left_high_point = tem;
                        continuous =0;
                        repaired.clear();
                    }
                    else if(tem.low_thres_pulsewidth < 15000)
                    {

                        // 找到每个repaired点的上下点
                        bool up_down_flag = true;
                        //OnePointInfo UpPoint;
                        OnePointInfo down_point;

                        if((line_id-1) > 0 && line_id+1 <= 144)
                        {
                            //UpPoint = points.GetData(1,line_id+1,angle);
                            down_point = points.GetData(1,line_id-1,angle);

                            if( /*(UpPoint.x ==0 && UpPoint.y ==0 && UpPoint.z ==0) ||*/
                                (down_point.x ==0 && down_point.y ==0 && down_point.z ==0) ||
                                //                        UpPoint.low_thres_pulsewidth < 25000 ||
                                down_point.low_thres_pulsewidth < 25000)
                            {
                                up_down_flag = false;
                            }
                        }

                        if(up_down_flag == false)
                        {
                            up_down_flag = true;
                            if((line_id-3) > 0 && line_id+1 <= 144)
                            {
                                //UpPoint = points.GetData(1,line_id+1,angle);
                                down_point = points.GetData(1,line_id-3,angle);

                                if( /*(UpPoint.x ==0 && UpPoint.y ==0 && UpPoint.z ==0) ||*/
                                    (down_point.x ==0 && down_point.y ==0 && down_point.z ==0) ||
                                    //                        UpPoint.low_thres_pulsewidth < 25000 ||
                                    down_point.low_thres_pulsewidth < 25000)
                                {
                                    up_down_flag = false;
                                }
                            }
                        }

                        if(up_down_flag)
                        {
                            OnePointInfo p1;
                            OnePointInfo p2;
                            bool ok = false;
                            if(((up_down_flag)/*&&(std::abs(UpPoint.y - down_point.y)  <= 1)*/))
                            {
                                //p1 = UpPoint;
                                p2 = down_point;
                                ok = true;
                            }
                            if(p1.line_id == 0 && p2.line_id == 0)
                            {
                                ok = false;
                            }
                            else if(p1.line_id == 0)
                            {
                                p1 = p2;
                            }
                            else if(p2.line_id == 0)
                            {
                                p2 = p1;
                            }
                            else
                            {
                                ok = false;
                            }

                            if(ok)
                            {
                                int diff = std::abs((int)p1.distance - (int)tem.distance);
                                int tong_idx = -1;
                                for(int i = 0 ; i < repaired.size();i++)
                                {
                                    // 检测和当前点相同匹配的点
                                    if(std::abs((int)repaired[i].index - (int)tem.index) == 1 && (repaired[i].low_thres_pulsewidth == tem.low_thres_pulsewidth))
                                    {
                                        tong_idx = i;
                                    }
                                }
                                if(diff >= 38000 && diff <= 42000)
                                {
                                    DoublePointAvg(tem,p1,p2);
                                    if(tong_idx != -1)
                                    {
                                        repaired[tong_idx].distance = tem.distance;
                                        repaired[tong_idx].x = tem.x;
                                        repaired[tong_idx].y = tem.y;
                                        repaired[tong_idx].z = tem.z;
                                        repaired[tong_idx].blank_point_compen = 1;
                                        //if(!BlankPointCompenHighlight)
                                        //{
                                            repaired[tong_idx].r = tem.r;
                                            repaired[tong_idx].g = tem.g;
                                            repaired[tong_idx].b = tem.b;
                                        // }
                                        // else
                                        // {
                                        //     repaired[tong_idx].r = 255;
                                        //     repaired[tong_idx].g = 0;
                                        //     repaired[tong_idx].b = 0;
                                        // }
                                        points.insert(repaired[tong_idx].index,repaired[tong_idx]);
                                        repaired.erase(repaired.begin()+ tong_idx);
                                    }

                                    tem.blank_point_compen = 1;
                                    //if(!BlankPointCompenHighlight)
                                    //{
                                        tem.r = (p2.r + p1.r) / 2;
                                        tem.g = (p2.g + p1.g) / 2;
                                        tem.b = (p2.b + p1.b) / 2;
                                    // }
                                    // else
                                    // {
                                    //     tem.r = 255;
                                    //     tem.g = 0;
                                    //     tem.b = 0;
                                    // }

                                    points.insert(tem.index,tem);
                                    //continuous = 2;
                                    //repaired.clear();
                                    continue;
                                }
                            }

                        }


                        if(continuous == 0 || continuous == 1)
                        {
                            //-- 待检测
                            repaired.push_back(tem);
                            continuous = 1;
                        }
                        if(repaired.size() >= 6)
                        {
                            continuous = 2;
                            repaired.clear();
                        }
                    }
                }
            }
        }

        inline void Filterate::_40BlankPointCompenProcessV6(OnePointInfoManage &points)
        {
            // 1 9 17 25 33 41
            static const int check_line_id[] = { 1, 2, 3,
                                            25, 26, 27,
                                            49, 50, 51,
                                            73, 74, 75,
                                            97, 98, 99,
                                            121, 122, 123};
            static OnePointInfo left_point; // 左边点
            static OnePointInfo right_point; // 右边点
            static int points_weak_flag = 0;
            static int points_weak_cn = 0;
            static int config_continuous_zero_cn= 5; // 连续5个点为 0 则不进行补点，--> 因此 只支持 4个点为 0 值 才能补
            static int config_interval_distance = 200; // 20cm
            if(h_resolution_ == 1)
            {
                config_continuous_zero_cn= 15;
            }
            else
            {
                config_continuous_zero_cn= 30;
            }

            int line_id = 1;
            for(int i = 0; i < 18;i++)
            {
                line_id = check_line_id[i];

                for(int angle = start_angle_; angle < 1200;angle+=h_resolution_)
                {
                    OnePointInfo& point = points.GetData(1,line_id,angle);

                    // 130米后的鬼影
                    if(point.distance > 130000)
                    {
                        if(point.low_thres_pulsewidth > 19000) // 脉宽大于 19ns 检查同时发光的LD
                        {
                            std::vector<int> lines = corrspond_line_[point.ld_id-1];
                            //int MirrorNo = tem.Mirror;
                            int h_angle = point.h_angle;
                            for(int idx = 0;idx < lines.size();idx++)
                            {
                                if(point.line_id != lines[idx] && (lines[idx] > 85)) // 自己就不用检测了
                                {
                                    long Key = points.GetIndex(1,lines[idx],h_angle);
                                    if(Key == -1)
                                        continue;
                                    auto& p = points.GetDataIt(Key);

                                    if(p.low_thres_pulsewidth < 9000 && std::abs((int)p.distance - (int)point.distance) < 300)
                                    {
                                        // if(GhostHighlight)
                                        // {
                                        //     p.r = GhostHighlightColor.redF();
                                        //     p.g = GhostHighlightColor.greenF();
                                        //     p.b = GhostHighlightColor.blueF();
                                        // }
                                        // else
                                        {
                                            p.distance = 0;
                                            p.x = 0;
                                            p.y = 0;
                                            p.z = 0;
                                        }
                                    }
                                }
                            }
                        }
                    }


                    if(point.distance == 0)
                        continue;
                    if(point.low_thres_pulsewidth >= 25000)
                    {
                        if(points_weak_flag == 1) // 表示两个高反点之间存在弱反
                        {
                            right_point = point;
                            // 判断 和 左点间隔 多少个 点
                            int interval_index = right_point.index - left_point.index;
                            int interval_distance = std::abs((int)right_point.distance - (int)left_point.distance);
                            int next_point_angle = left_point.h_angle + h_resolution_;

                            if( (interval_index > 0) &&
                                (interval_index < config_continuous_zero_cn) &&
                                (interval_distance < config_interval_distance))
                            {
                                int avg_distance = (left_point.distance + right_point.distance) /2;
                                int avg_pulsewidth = (left_point.low_thres_pulsewidth + right_point.low_thres_pulsewidth) /2;
                                int intensity = (left_point.intensity + right_point.intensity) /2;
                                float r = (left_point.r + right_point.r) / 2;
                                float g = (left_point.g + right_point.g) / 2;
                                float b = (left_point.b + right_point.b) / 2;

                                for(int i = 0; i < interval_index; i+= h_resolution_)
                                {
                                    OnePointInfo& src = points.GetData(1,point.line_id,next_point_angle);
                                    if(src.line_id == 0)
                                        continue;
                                    int flag = false;
                                    int diff = right_point.distance - src.old_distance;
                                    if(diff >= 38000 && diff <= 42000 && src.low_thres_pulsewidth < 15000)
                                    {
                                        flag = true;
                                    }
                                    else
                                    {
                                        //                                // 检查下点
                                        //                                int down_one_point_line = (int)point.line_id-1;
                                        //                                int down_three_point_line = (int)point.line_id-3;
                                        //                                if((down_one_point_line >= 1) && (down_one_point_line <= 144))
                                        //                                {
                                        //                                    OnePointInfo& down_point = points.GetData(1,point.line_id - 1,next_point_angle);
                                        //                                    if( down_point._40_blank_point_compen == 0 &&
                                        //                                        down_point.low_thres_pulsewidth > 25000 &&
                                        //                                        down_point.distance >0)
                                        //                                    {
                                        //                                        flag = true;
                                        //                                    }
                                        //                                }
                                        //                                else if( (down_three_point_line >= 1) && (down_three_point_line <= 144))
                                        //                                {
                                        //                                    OnePointInfo& down_point = points.GetData(1,point.line_id - 3,next_point_angle);
                                        //                                    if( down_point._40_blank_point_compen == 0 &&
                                        //                                        down_point.low_thres_pulsewidth > 25000 &&
                                        //                                        down_point.distance > 0)
                                        //                                    {
                                        //                                        flag = true;
                                        //                                    }
                                        //                                }
                                    }

                                    if(flag)
                                    {
                                        double cos_rid_sin = COS((int)src.vazimuth) * SIN((int)src.azimuth * 10) *0.001;
                                        double cos_rid_cos = COS((int)src.vazimuth) * COS((int)src.azimuth * 10) *0.001;
                                        double sin_rid_thousandth = SIN((int)src.vazimuth) *0.001;
                                        src.distance = avg_distance;
                                        src.low_thres_pulsewidth = avg_pulsewidth;
                                        src.intensity = intensity;
                                        src.y = src.distance * cos_rid_sin;//新坐标系计算方式，逆时针改顺时针的方式
                                        src.x = src.distance * cos_rid_cos;
                                        src.z =  src.distance * sin_rid_thousandth;//不补偿
                                        src._40_blank_point_compen = 1;
                                        // if(!BlankPointCompenHighlight)
                                        // {
                                        //    src.r = r;
                                        //    src.g = g;
                                        //    src.b = b;
                                        // }
                                        // else
                                        // {
                                        //     src.r = 255;
                                        //     src.g = 0;
                                        //     src.b = 0;
                                        // }
                                    }

                                    next_point_angle += h_resolution_;
                                }

                            }
                            left_point = point;
                            points_weak_flag = 0;
                            points_weak_cn = 0;
                        }
                        else
                        {
                            left_point = point;
                            points_weak_flag = 0;
                            points_weak_cn = 0;
                        }
                    }
                    else if(point.low_thres_pulsewidth < 15000)
                    {
                        points_weak_flag = 1;


                        int flag = false;
                        // 检查下点
                        int down_one_point_line = (int)point.line_id-1;
                        int down_three_point_line = (int)point.line_id-3;
                        OnePointInfo down_point;
                        if((down_one_point_line >= 1) && (down_one_point_line <= 144))
                        {
                            int tem_angle = angle;
                            if(point.line_id == 97)
                                tem_angle-=38;
                            if(tem_angle < 0)
                                continue;
                            down_point = points.GetData(1,point.line_id - 1,tem_angle);
                            if( down_point._40_blank_point_compen == 0 &&
                                down_point.low_thres_pulsewidth > 25000 &&
                                down_point.distance >0)
                            {
                                flag = true;
                            }
                        }
                        else if( (down_three_point_line >= 1) && (down_three_point_line <= 144))
                        {
                            int tem_angle = angle;
                            if(point.line_id == 97)
                                tem_angle-=38;
                            if(tem_angle < 0)
                                continue;
                            down_point = points.GetData(1,point.line_id - 3,tem_angle);
                            if( down_point._40_blank_point_compen == 0 &&
                                down_point.low_thres_pulsewidth > 25000 &&
                                down_point.distance > 0)
                            {
                                flag = true;
                            }
                        }
                        if(flag)
                        {
                            OnePointInfo& src = point;

                            int avg_distance = down_point.distance;
                            int avg_pulsewidth = down_point.low_thres_pulsewidth;
                            int intensity = down_point.intensity;
                            float r = down_point.r;
                            float g = down_point.g;
                            float b = down_point.b;


                            double cos_rid_sin = COS((int)src.vazimuth) * SIN((int)src.azimuth * 10) *0.001;
                            double cos_rid_cos = COS((int)src.vazimuth) * COS((int)src.azimuth * 10) *0.001;
                            double sin_rid_thousandth = SIN((int)src.vazimuth) *0.001;
                            src.old_distance = src.distance;
                            src.distance = avg_distance;
                            src.low_thres_pulsewidth = avg_pulsewidth;
                            src.intensity = intensity;
                            src.y = src.distance * cos_rid_sin;//新坐标系计算方式，逆时针改顺时针的方式
                            src.x = src.distance * cos_rid_cos;
                            src.z =  src.distance * sin_rid_thousandth;//不补偿
                            src._40_blank_point_compen = 1;
                            // if(!BlankPointCompenHighlight)
                            // {
                            //     src.r = r;
                            //     src.g = g;
                            //     src.b = b;
                            // }
                            // else
                            // {
                            //     src.r = 255;
                            //     src.g = 0;
                            //     src.b = 0;
                            // }
                        }

                        //                if(points_weak_flag == 1)
                        //                {
                        //                    points_weak_cn++;

                        //                    // 如果超过了 配置 补点最大值 则  不能拉点
                        //                    if(points_weak_cn >= config_continuous_zero_cn)
                        //                    {
                        //                        points_weak_cn = 0; // 清空 标志位
                        //                        points_weak_cn = 0;   // 清空 连续 0点次数
                        //                    }
                        //                }
                    }


                }
            }
        }

        inline void Filterate::ExpansionProcess(OnePointInfoManage& points)
        {

            std::vector<OnePointInfo> tem_points;
            for(int line_id = 1; line_id <= 144;line_id++)
            {
                tem_points.clear();

                for(int angle = 1; angle < 1200;angle+=h_resolution_)
                {
                    OnePointInfo& point = points.GetData(1,line_id,angle);
                    if(point.low_thres_pulsewidth > 29000)
                    {
                        tem_points.push_back(point);
                    }
                }

                int size = tem_points.size();
                if(size == 0)
                    continue;

                int pre_angle = tem_points[0].azimuth;
                int pre_h_angle = tem_points[0].h_angle;
                int sum = 0;
                int start_angle_ = 0;
                int end_angle = 0;
                int dis = 0;
                double z_dis = 0;
                for(int i = 0 ; i < size; i++)
                {
                    int diff = tem_points[i].azimuth - pre_angle;
                    if( (diff <= 50) && (diff>=20) )
                    {
                        sum++;
                        dis += tem_points[i].distance;
                        z_dis += tem_points[i].z;
                        tem_points[i].non_filterable = 1;
                        points.insert(tem_points[i].index,tem_points[i]);
                        if(start_angle_ == 0)
                            start_angle_ = tem_points[i].h_angle;
                        end_angle = tem_points[i].h_angle;

                        while(++pre_h_angle)
                        {
                            if(pre_h_angle >= tem_points[i].h_angle)
                            {
                                break;
                            }
                            OnePointInfo& tem_point = points.GetData(1,line_id,pre_h_angle);
                            if(tem_point.distance == 0)
                                continue;
                            tem_point.non_filterable =1;
                        }

                    }
                    else if(diff <= 50 && (i != size-1))
                    {
                        sum++;
                        dis += tem_points[i].distance;
                        z_dis += tem_points[i].z;
                        if(start_angle_ == 0)
                            start_angle_ = tem_points[i].h_angle;
                        end_angle = tem_points[i].h_angle;
                    }
                    else if(diff > 50 || (i == size-1))
                    {
                        if(sum > 25)
                        {
                            dis /= sum;
                            z_dis /= sum;
                            // 检查上下4条线的交集比
                            if( (line_id - 4 >= 1) && (line_id - 4 <= 144) &&
                                (line_id + 4 >= 1) && (line_id + 4 <= 144))
                            {

                                int tem_start_angle = start_angle_;
                                int tem_end_angle = end_angle;
                                // 如果 当前线是 45 ~ 46那么-4后 角度 要 -38
                                if(line_id >= 45 && line_id <= 48)
                                {
                                    if( ((tem_start_angle - 38 >= 1) && (tem_start_angle - 38 < 1200)) &&
                                        ((tem_end_angle - 38) >= 1) && (tem_end_angle - 38 < 1200))
                                    {
                                        tem_start_angle -= 38;
                                        tem_end_angle -= 38;
                                    }
                                    else
                                    {
                                        continue;
                                    }
                                }
                                else if(line_id >= 93 && line_id <= 96)
                                {
                                    if( ((tem_start_angle + 38 >= 1) && (tem_start_angle + 38 < 1200)) &&
                                        ((tem_end_angle + 38) >= 1) && (tem_end_angle + 38 < 1200))
                                    {
                                        tem_start_angle += 38;
                                        tem_end_angle += 38;
                                    }
                                    else
                                    {
                                        continue;
                                    }
                                }


                                int sum1 = 0;
                                int sum2 = 0;
                                int inspect_line = line_id + 4;
                                int inspect_line1 = line_id - 4;
                                int tem_pre_angle = points.GetData(1,inspect_line,tem_start_angle).azimuth;
                                for(int j = tem_start_angle; j < tem_end_angle; j+=h_resolution_)
                                {
                                    OnePointInfo& tem_point = points.GetData(1,inspect_line,j);
                                    int diff1 = tem_point.azimuth - tem_pre_angle;
                                    if( (diff1 <= 50) && (diff1>=2) )
                                    {
                                        sum1++;
                                    }
                                    else if(diff1 <= 50)
                                    {
                                        sum1++;
                                    }
                                    else if(diff1 > 50)
                                    {
                                        break;
                                    }
                                    tem_pre_angle = tem_point.azimuth;
                                }
                                if(sum1 >= sum *0.7)
                                {
                                    tem_pre_angle = points.GetData(1,inspect_line1,tem_start_angle).azimuth;
                                    for(int j = tem_start_angle; j < tem_end_angle; j+=h_resolution_)
                                    {
                                        OnePointInfo& tem_point = points.GetData(1, inspect_line1, j);
                                        int diff1 = tem_point.azimuth - tem_pre_angle;
                                        if( (diff1 <= 50) && (diff1>=2) )
                                        {
                                            sum2++;
                                        }
                                        else if(diff1 <= 50)
                                        {
                                            sum2++;
                                        }
                                        else if(diff1 > 50)
                                        {
                                            break;
                                        }
                                        tem_pre_angle = tem_point.azimuth;
                                    }
                                    if(sum2 >= sum *0.7)
                                    {
                                        // 上下 10条线
                                        int up_line = line_id;
                                        int down_line = line_id;
                                        double tem_z_dis = z_dis;
                                        double next_z_dis = 0;
                                        while( ++up_line <= 144)
                                        {
                                            tem_start_angle = start_angle_ - 20;
                                            tem_start_angle = tem_start_angle < 1? 1:tem_start_angle;
                                            tem_end_angle = end_angle + 20;
                                            tem_end_angle = tem_end_angle > 1200 ? 1200:tem_end_angle;

                                            if(line_id >= 1 && line_id <= 48)
                                            {
                                                if(up_line >= 1 && up_line <= 48)
                                                {

                                                }
                                                else if(up_line >= 49 && up_line <= 96)
                                                {
                                                    tem_start_angle -= 38;
                                                    tem_end_angle -= 38;
                                                }
                                                else if(up_line >= 97 && up_line <= 144)
                                                {

                                                }
                                            }
                                            else if(line_id >= 49 && line_id <= 96)
                                            {
                                                if(up_line >= 1 && up_line <= 48)
                                                {
                                                    tem_start_angle += 38;
                                                    tem_end_angle += 38;
                                                }
                                                else if(up_line >= 49 && up_line <= 96)
                                                {

                                                }
                                                else if(up_line >= 97 && up_line <= 144)
                                                {
                                                    tem_start_angle += 38;
                                                    tem_end_angle += 38;
                                                }
                                            }
                                            else if(line_id >= 97 && line_id <= 144)
                                            {
                                                if(up_line >= 1 && up_line <= 48)
                                                {

                                                }
                                                else if(up_line >= 49 && up_line <= 96)
                                                {
                                                    tem_start_angle -= 38;
                                                    tem_end_angle -= 38;
                                                }
                                                else if(up_line >= 97 && up_line <= 144)
                                                {

                                                }
                                            }
                                            if(tem_start_angle < 1 || tem_start_angle>=1200)
                                                break;
                                            if(tem_end_angle < 1 || tem_end_angle>=1200)
                                                break;

                                            int cn =0 ;
                                            for(int j = tem_start_angle; j < tem_end_angle; j+=h_resolution_)
                                            {

                                                OnePointInfo& tem_point = points.GetData(1, up_line, j);
                                                if(tem_point.distance == 0)
                                                    continue;
        //                                        if((std::abs(tem_z_dis - tem_point.z) > 2) &&
        //                                            (j > tem_start_angle+30) &&
        //                                            (j < tem_end_angle - 30))
        //                                        {
        //                                            break;
        //                                        }

                                                if( (std::abs(dis - (int)tem_point.distance) <= 150) &&
                                                    (tem_point.low_thres_pulsewidth < 16000) &&
                                                    (tem_point.non_filterable == 0))
                                                {
                                                    next_z_dis += tem_point.z;
                                                    cn++;
                                                    tem_point.expansion = 1;

                                                    // if(ExpansionHighlinght)
                                                    // {
                                                    //     tem_point.r = 255;
                                                    //     tem_point.g = 0;
                                                    //     tem_point.b = 255;
                                                    // }
                                                    //else
                                                    {
                                                        tem_point.x = 0;
                                                        tem_point.y = 0;
                                                        tem_point.z = 0;
                                                        tem_point.distance = 0;
                                                    }
                                                }
                                            }

                                            if(cn > 0)
                                            {
                                                next_z_dis /= cn;
                                                tem_z_dis = next_z_dis;
                                            }


                                            if(line_id+10 == up_line)
                                                break;
                                        }

                                        tem_z_dis = z_dis;
                                        next_z_dis = 0;
                                        while( --down_line >= 1)
                                        {
                                            tem_start_angle = start_angle_ - 20;
                                            tem_start_angle = tem_start_angle < 1? 1:tem_start_angle;
                                            tem_end_angle = end_angle + 20;
                                            tem_end_angle = tem_end_angle > 1200 ? 1200:tem_end_angle;

                                            if(line_id >= 1 && line_id <= 48)
                                            {
                                                if(down_line >= 1 && down_line <= 48)
                                                {

                                                }
                                                else if(down_line >= 49 && down_line <= 96)
                                                {
                                                    tem_start_angle -= 38;
                                                    tem_end_angle -= 38;
                                                }
                                                else if(down_line >= 97 && down_line <= 144)
                                                {

                                                }
                                            }
                                            else if(line_id >= 49 && line_id <= 96)
                                            {
                                                if(down_line >= 1 && down_line <= 48)
                                                {
                                                    tem_start_angle += 38;
                                                    tem_end_angle += 38;
                                                }
                                                else if(down_line >= 49 && down_line <= 96)
                                                {

                                                }
                                                else if(down_line >= 97 && down_line <= 144)
                                                {
                                                    tem_start_angle += 38;
                                                    tem_end_angle += 38;
                                                }
                                            }
                                            else if(line_id >= 97 && line_id <= 144)
                                            {
                                                if(down_line >= 1 && down_line <= 48)
                                                {

                                                }
                                                else if(down_line >= 49 && down_line <= 96)
                                                {
                                                    tem_start_angle -= 38;
                                                    tem_end_angle -= 38;
                                                }
                                                else if(down_line >= 97 && down_line <= 144)
                                                {

                                                }
                                            }
                                            if(tem_start_angle < 1 || tem_start_angle>=1200)
                                                break;
                                            if(tem_end_angle < 1 || tem_end_angle>=1200)
                                                break;

                                            int cn = 0;
                                            for(int j = tem_start_angle; j < tem_end_angle; j+=h_resolution_)
                                            {
                                                OnePointInfo& tem_point = points.GetData(1, down_line, j);
                                                if(tem_point.distance == 0)
                                                    continue;
        //                                        if((std::abs(tem_z_dis - tem_point.z) > 2) &&
        //                                            (j > tem_start_angle+30) &&
        //                                            (j < tem_end_angle - 30))
        //                                        {
        //                                            break;
        //                                        }

                                                if( (std::abs(dis - (int)tem_point.distance) <= 150) &&
                                                    (tem_point.low_thres_pulsewidth < 16000) &&
                                                    (tem_point.non_filterable == 0))
                                                {
                                                    tem_point.expansion = 1;

                                                    next_z_dis += tem_point.z;
                                                    cn++;

                                                    // if(ExpansionHighlinght)
                                                    // {
                                                    //     tem_point.r = 255;
                                                    //     tem_point.g = 0;
                                                    //     tem_point.b = 255;
                                                    // }
                                                    // else
                                                    {
                                                        tem_point.x = 0;
                                                        tem_point.y = 0;
                                                        tem_point.z = 0;
                                                        tem_point.distance = 0;
                                                    }
                                                }
                                            }

                                            if(cn > 0)
                                            {
                                                next_z_dis /= cn;
                                                tem_z_dis = next_z_dis;
                                            }


                                            if(line_id-10 == down_line)
                                                break;
                                        }
                                    }
                                }
                            }

                        }

                        sum = 0;
                        start_angle_ = 0;
                        end_angle = 0;
                        dis = 0;
                    }

                    pre_angle = tem_points[i].azimuth;
                    pre_h_angle = tem_points[i].h_angle;
                }
            }

        }

        inline void Filterate::DoublePointAvg(OnePointInfo& src, OnePointInfo& p1,OnePointInfo& p2)
        {
            int left_add_right_avg = (p2.distance + p1.distance) /2;
            int left_add_right_pulsewidth_avg = (p2.low_thres_pulsewidth + p1.low_thres_pulsewidth) /2;
            int intensity = (p2.intensity + p1.intensity) /2;
            double cos_rid_sin = COS((int)src.vazimuth) * SIN((int)src.azimuth * 10) *0.001;
            double cos_rid_cos = COS((int)src.vazimuth) * COS((int)src.azimuth * 10) *0.001;
            double sin_rid_thousandth = SIN((int)src.vazimuth) *0.001;
            src.distance = left_add_right_avg;
            src.low_thres_pulsewidth = left_add_right_pulsewidth_avg;
            src.intensity = intensity;
            src.y = src.distance * cos_rid_sin;//新坐标系计算方式，逆时针改顺时针的方式
            src.x = src.distance * cos_rid_cos;
            src.z =  src.distance * sin_rid_thousandth;//不补偿
        }
    
        inline void Filterate::Angle38Check(int& angle1, int& angle2, int cuLine, int check_line)
        {
            if(cuLine >= 1 && cuLine <= 48)
            {
                if(check_line >= 1 && check_line <= 48)
                {

                }
                else if(check_line >= 49 && check_line <= 96)
                {
                    angle1 -= 38;
                    angle2 -= 38;
                }
                else if(check_line >= 97 && check_line <= 144)
                {

                }
            }
            else if(cuLine >= 49 && cuLine <= 96)
            {
                if(check_line >= 1 && check_line <= 48)
                {
                    angle1 += 38;
                    angle2 += 38;
                }
                else if(check_line >= 49 && check_line <= 96)
                {

                }
                else if(check_line >= 97 && check_line <= 144)
                {
                    angle1 += 38;
                    angle2 += 38;
                }
            }
            else if(cuLine >= 97 && cuLine <= 144)
            {
                if(check_line >= 1 && check_line <= 48)
                {

                }
                else if(check_line >= 49 && check_line <= 96)
                {
                    angle1 -= 38;
                    angle2 -= 38;
                }
                else if(check_line >= 97 && check_line <= 144)
                {

                }
            }
        }

        inline void Filterate::FilterNoiseV2(OnePointInfoManage &points,OnePointInfoManage &pre_points)
        {
            std::vector<OnePointInfo> check_point;
            // 只管5米后的点
            for(int line_id = 1; line_id <= 144;line_id++)
            {
                for(int angle = start_angle_; angle < 1200;angle+= h_resolution_ * 2)
                {
                    OnePointInfo& pre_point = pre_points.GetData(1,line_id,angle);
                    OnePointInfo& point = points.GetData(1,line_id,angle);


                    if(point.distance < full_search_scope_)
                    {
                        continue;
                    }

                    if(point.line_id == 69 && point.h_angle == 755)
                    {
                        int debug = 0;
                    }

                    if(point.line_id == 0 || pre_point.line_id == 0 || point.time - pre_point.time > 300)
                    {
                        if(point.line_id != 0)
                        {
                            check_point.push_back(point);
                        }
                        else
                        {
                            point.non_filterable = 1;
                            if(h_resolution_ == 1)
                            {
                                OnePointInfo& mate_point = points.GetData(1,line_id,angle+1);
                                if(mate_point.line_id != 0)
                                {
                                    mate_point.non_filterable = 1;
                                }
                            }
                        }

                        continue;
                    }

                    if( std::abs((long)point.distance - (long)pre_point.distance) > 2500)
                    {
                        check_point.push_back(point);
                    }
                    else
                    {
                        point.non_filterable = 1;
                        if(h_resolution_ == 1)
                        {
                            OnePointInfo& mate_point = points.GetData(1,line_id,angle+1);
                            if(mate_point.line_id != 0)
                            {
                                mate_point.non_filterable = 1;
                            }
                        }
                    }
                }
            }


            int cnt = 0;
            for(int i =0; i < (int)check_point.size(); i++)
            {
                OnePointInfo& point = check_point[i];
                int line_id = point.line_id;
                if(point.distance == 0)
                    continue;
                if(point.line_id == 69 && point.h_angle == 755)
                {
                    int debug = 0;
                }
                cnt = 0;
                // 查看上下5线
                int check_line_start = (std::max)(point.line_id - 5, 1);
                int check_line_end = (std::min)(point.line_id + 5, 144);
                for(int check_line = check_line_start; check_line < check_line_end; check_line++)
                {

                    int find_left_angle = point.h_angle - 20;
                    int find_right_angle = point.h_angle + 20;


                    if(line_id >= 1 && line_id <= 48)
                    {
                        if(check_line >= 1 && check_line <= 48)
                        {

                        }
                        else if(check_line >= 49 && check_line <= 96)
                        {
                            find_left_angle -= 38;
                            find_right_angle -= 38;
                        }
                        else if(check_line >= 97 && check_line <= 144)
                        {

                        }
                    }
                    else if(line_id >= 49 && line_id <= 96)
                    {
                        if(check_line >= 1 && check_line <= 48)
                        {
                            find_left_angle += 38;
                            find_right_angle += 38;
                        }
                        else if(check_line >= 49 && check_line <= 96)
                        {

                        }
                        else if(check_line >= 97 && check_line <= 144)
                        {
                            find_left_angle += 38;
                            find_right_angle += 38;
                        }
                    }
                    else if(line_id >= 97 && line_id <= 144)
                    {
                        if(check_line >= 1 && check_line <= 48)
                        {

                        }
                        else if(check_line >= 49 && check_line <= 96)
                        {
                            find_left_angle -= 38;
                            find_right_angle -= 38;
                        }
                        else if(check_line >= 97 && check_line <= 144)
                        {

                        }
                    }


                    find_left_angle = find_left_angle < start_angle_ ? start_angle_ : find_left_angle;
                    find_right_angle = find_right_angle > 1200? 1200 : find_right_angle;

                    double outlier_radius_pow = outlier_radius_ * outlier_radius_;

                    for(int ag = find_left_angle; ag < find_right_angle; ag += h_resolution_ * 2)
                    {
                        if(ag == point.h_angle && point.line_id == check_line)
                            continue;

                        OnePointInfo& tem = points.GetData(1,check_line,ag);
                        if(tem.distance == 0)
                            continue;

        //                float pArray[] __attribute__ ((aligned (16))) = { 0, 0, 0, 0};
        //                pArray[0] = tem.x - point.x;
        //                pArray[1] = tem.y - point.y;
        //                pArray[2] = tem.z - point.z;
        //                DWORD dwGroupCount = 4 / 4;
        //                __m128 e_Scale = _mm_set_ps1(fScale);//设置所有4个值为同一值

        //                for (DWORD i = 0; i < dwGroupCount; i++)
        //                {
        //                    *(__m128*)(pArray + i * 4) = _mm_sqrt_ps(e_Scale);
        //                }


                        double val = (std::pow(tem.x - point.x,2) + std::pow(tem.y - point.y,2) + std::pow(tem.z - point.z,2));
                        if(val <= outlier_radius_pow)
                        {
                            cnt++;
                            if(cnt >= outlier_num_)
                            {
                                points.GetData(1,line_id,point.h_angle).non_filterable = 1;
                                if(h_resolution_ == 1)
                                {
                                    OnePointInfo& mate_point = points.GetData(1,line_id,point.h_angle+1);
                                    if(mate_point.line_id != 0)
                                    {
                                        mate_point.non_filterable = 1;
                                    }
                                }
                                break;
                            }
                        }
                    }
                    if(cnt >= outlier_num_)
                        break;
                }
            }
        }
    
        inline void Filterate::FilterNoiseProcess(int init, OnePointInfo &point,OnePointInfo &pre_point,OnePointInfoManage &points)
        {
            static std::vector<OnePointInfo> check_point;
            if(!outlier_enable_)
                return;


            if(init == 1)
            {
                int cnt = 0;
                for(int i =0; i < (int)check_point.size(); i++)
                {
                    OnePointInfo& point = check_point[i];
                    int line_id = point.line_id;
                    if(point.distance == 0)
                        continue;

                    if(point.line_id == 126 && point.h_angle == 189)
                    {
                        int debug = 0;
                    }

                    cnt = 0;
                    // 查看上下5线
                    int check_line_start = (std::max)(point.line_id - 5, 1);
                    int check_line_end = (std::min)(point.line_id + 5, 144);
                    for(int check_line = check_line_start; check_line < check_line_end; check_line++)
                    {

                        int find_left_angle = point.h_angle - 20;
                        int find_right_angle = point.h_angle + 20;

                        Angle38Check(find_left_angle, find_right_angle, line_id, check_line);


                        find_left_angle = find_left_angle < start_angle_ ? start_angle_ : find_left_angle;
                        find_right_angle = find_right_angle > 1200? 1200 : find_right_angle;

                        double outlier_radius_pow = outlier_radius_ * outlier_radius_;

                        for(int ag = find_left_angle; ag < find_right_angle; ag += h_resolution_ * 2)
                        {
                            if(ag == point.h_angle && point.line_id == check_line)
                                continue;

                            OnePointInfo& tem = points.GetData(point.echo_type,check_line,ag);
                            if(tem.distance == 0)
                                continue;

                            double val = (std::pow(tem.x - point.x,2) + std::pow(tem.y - point.y,2) + std::pow(tem.z - point.z,2));
                            if(val <= outlier_radius_pow)
                            {
                                cnt++;
                                if(cnt >= outlier_num_)
                                {
                                    points.GetData(point.echo_type,line_id,point.h_angle).non_filterable = 1;
                                    if(h_resolution_ == 1)
                                    {
                                        OnePointInfo& mate_point = points.GetData(point.echo_type,line_id,point.h_angle+1);
                                        if(mate_point.line_id != 0)
                                        {
                                            mate_point.non_filterable = 1;
                                        }
                                    }
                                    break;
                                }
                            }
                        }
                        if(cnt >= outlier_num_)
                            break;
                    }
                }

                check_point.clear();
                return;
            }


            if(start_angle_ % 2 == 0)
            {
                if((int)point.h_angle % 2 != 0)
                {
                    return;
                }
            }
            else
            {
                if((int)point.h_angle % 2 == 0)
                {
                    return;
                }
            }

            if(point.distance < full_search_scope_)
            {
                return;
            }


            if(point.line_id == 0 || pre_point.line_id == 0 || point.time - pre_point.time > 300)
            {
                if(point.line_id != 0)
                {
                    check_point.push_back(point);
                }
                else
                {
                    point.non_filterable = 1;
                    if(h_resolution_ == 1)
                    {
                        OnePointInfo& mate_point = points.GetData(point.echo_type,point.line_id,point.h_angle+1);
                        if(mate_point.line_id != 0)
                        {
                            mate_point.non_filterable = 1;
                        }
                    }
                }

                return;
            }

            if( std::abs((long)point.distance - (long)pre_point.distance) > 2500)
            {
                check_point.push_back(point);
            }
            else
            {
                point.non_filterable = 1;
                if(h_resolution_ == 1)
                {
                    OnePointInfo& mate_point = points.GetData(point.echo_type,point.line_id,point.h_angle+1);
                    if(mate_point.line_id != 0)
                    {
                        mate_point.non_filterable = 1;
                    }
                }
            }
        }

        // 近距离噪点
        inline void Filterate::close_range_outliers(OnePointInfoManage &points, OnePointInfoManage &pre_points)
        {

            int cnt = 0;
            for(int line_id = 1; line_id <= 144;line_id++)
            {
                for(int angle = start_angle_; angle < 1200;angle+=h_resolution_)
                {

                    OnePointInfo& point = points.GetData(1,line_id,angle);

                    if(point.distance < full_search_scope_)
                    {

                        if(point.distance == 0)
                            continue;


                        cnt = 0;

                        // 查看上下5线
                        int check_line_start = (std::max)(point.line_id - 5, 1);
                        int check_line_end = (std::min)(point.line_id + 5, 144);
                        for(int check_line = check_line_start; check_line < check_line_end; check_line++)
                        {

                            int find_left_angle = point.h_angle - 20;
                            int find_right_angle = point.h_angle + 20;


                            if(line_id >= 1 && line_id <= 48)
                            {
                                if(check_line >= 1 && check_line <= 48)
                                {

                                }
                                else if(check_line >= 49 && check_line <= 96)
                                {
                                    find_left_angle -= 38;
                                    find_right_angle -= 38;
                                }
                                else if(check_line >= 97 && check_line <= 144)
                                {

                                }
                            }
                            else if(line_id >= 49 && line_id <= 96)
                            {
                                if(check_line >= 1 && check_line <= 48)
                                {
                                    find_left_angle += 38;
                                    find_right_angle += 38;
                                }
                                else if(check_line >= 49 && check_line <= 96)
                                {

                                }
                                else if(check_line >= 97 && check_line <= 144)
                                {
                                    find_left_angle += 38;
                                    find_right_angle += 38;
                                }
                            }
                            else if(line_id >= 97 && line_id <= 144)
                            {
                                if(check_line >= 1 && check_line <= 48)
                                {

                                }
                                else if(check_line >= 49 && check_line <= 96)
                                {
                                    find_left_angle -= 38;
                                    find_right_angle -= 38;
                                }
                                else if(check_line >= 97 && check_line <= 144)
                                {

                                }
                            }


                            find_left_angle = find_left_angle < start_angle_ ? start_angle_ : find_left_angle;
                            find_right_angle = find_right_angle > 1200? 1200 : find_right_angle;

                            double square = close_range_outliers_radius_ * close_range_outliers_radius_;
                            for(int ag = find_left_angle; ag < find_right_angle; ag+=h_resolution_ * 2)
                            {
                                if(ag == point.h_angle && point.line_id == check_line)
                                    continue;

                                OnePointInfo& tem = points.GetData(1,check_line,ag);
                                if(tem.distance == 0)
                                    continue;
                                double x_diff = tem.x - point.x;
                                double y_diff = tem.y - point.y;
                                double z_diff = tem.z - point.z;
                                double val = (std::pow(x_diff,2) + std::pow(y_diff,2) + std::pow(z_diff,2));
                                if(val <= square)
                                {
                                    if(!(std::abs(x_diff) < 0.01 && std::abs(y_diff) < 0.01 && std::abs(z_diff) < 0.01))
                                    {
                                        cnt++;
                                        if(cnt >= close_range_outliers_num_)
                                        {
                                            point.close_range_outliers = 1;

                                            break;
                                        }
                                    }

                                }
                            }
                            if(cnt >= close_range_outliers_num_)
                                break;
                        }

                    }
                }
            }

        }
    
        inline void Filterate::CloseRangeOutliersProcess(int init, OnePointInfo& point,OnePointInfoManage &points)
        {
            static int cnt = 0;

            if(!close_range_outliers_enable_)
                return;

            if(point.distance < full_search_scope_)
            {
                if(point.distance == 0)
                    return;

                cnt = 0;

                // 查看上下5线
                int check_line_start = (std::max)(point.line_id - 5, 1);
                int check_line_end = (std::min)(point.line_id + 5, 144);
                for(int check_line = check_line_start; check_line < check_line_end; check_line++)
                {

                    int find_left_angle = point.h_angle - 20;
                    int find_right_angle = point.h_angle + 20;

                    Angle38Check(find_left_angle,find_right_angle,point.line_id,check_line);

                    find_left_angle = find_left_angle < start_angle_ ? start_angle_ : find_left_angle;
                    find_right_angle = find_right_angle > 1200? 1200 : find_right_angle;

                    double square = close_range_outliers_radius_ * close_range_outliers_radius_;
                    for(int ag = find_left_angle; ag < find_right_angle; ag+=h_resolution_ * 2)
                    {
                        if(ag == point.h_angle && point.line_id == check_line)
                            continue;

                        OnePointInfo& tem = points.GetData(point.echo_type,check_line,ag);
                        if(tem.distance == 0)
                            continue;
                        double x_diff = tem.x - point.x;
                        double y_diff = tem.y - point.y;
                        double z_diff = tem.z - point.z;
                        double val = (std::pow(x_diff,2) + std::pow(y_diff,2) + std::pow(z_diff,2));
                        if(val <= square)
                        {
                            if(!(std::abs(x_diff) < 0.01 && std::abs(y_diff) < 0.01 && std::abs(z_diff) < 0.01))
                            {
                                cnt++;
                                if(cnt >= close_range_outliers_num_)
                                {
                                    point.close_range_outliers = 1;

                                    break;
                                }
                            }
                        }
                    }
                    if(cnt >= close_range_outliers_num_)
                        break;
                }
            }

            if(init == 1)
            {
                cnt = 0;
            }
        }
    }
}

